#include <dispatch_server/vehicle_monitor.h>
#include <dispatch_server/dispatch_server.h>
#include <schedule_config/config.hpp>
#include <sch_common/sch_common.h>
#include <vector>


using namespace moying_proto;
using json = nlohmann::json;

#define ALLOW_MAX_NAV_FAIL_TIMES 2

namespace moying
{
namespace navigation
{
    VehicleMonitor::VehicleMonitor(const MoyingMapPtr &moyingmap, const VehiclePtr &vehicle, const visualization::VehicleVizPtr &viz, const communication::MoyingCommuPublishPtr &path_pub, const std::string &path_pub_topic)
    :nh_("vehicle_monitor_"+std::to_string(vehicle->getID())), 
    moyingmap_(moyingmap), 
    vehicle_(vehicle), 
    viz_(viz), 
    costmap_(moyingmap_->getCostMap()), 
    path_publisher_(path_pub), 
    path_pub_topic_(path_pub_topic),
    revise_vehicle_path_(vehicle,moyingmap)
    {
        initialize();
    }

    
    VehicleMonitor::~VehicleMonitor()
    {
        // waiter_.broadcast();
    }

    bool VehicleMonitor::initialize()
    {
        sch::ConfigInfo& cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
        if(moyingmap_ != nullptr)
            precheck_path_point_count_ = cfg_info.traffic_manager_cfg.collide_precheck_len / moyingmap_->getCostMap()->getResolution();
        else
            precheck_path_point_count_ = 3;
        
        result_.clear();
        last_try_recovery_time_ = std::chrono::system_clock::now()-std::chrono::seconds(100);
        path_follow_timeout_ = cfg_info.traffic_manager_cfg.path_follow_max_delay_seconds;
        precheck_one_way_road_can_pass_len_ = cfg_info.traffic_manager_cfg.precheck_one_way_road_can_pass_len;
        precheck_one_way_road_can_pass_len_ = precheck_one_way_road_can_pass_len_ <= 0 ? 3 : precheck_one_way_road_can_pass_len_;
    }

    bool VehicleMonitor::precheckPathPointValid(int* stop_path_index,int* collide_path_index){

        for(int i=0;i<precheck_path_point_count_;i++){
            int check_path_index = track_path_index_+i;
            if( !isPointValid(check_path_index) ){
                if(collide_path_index != nullptr){
                    *collide_path_index =check_path_index;
                }
                if(stop_path_index != nullptr){
                    // if(i > 2){
                    //     *stop_path_index = check_path_index-3;
                    // }else{
                    //     *stop_path_index = 0;
                    // }
                    if(i > precheck_path_point_count_-2){
                        *stop_path_index = check_path_index-4;
                        if(*stop_path_index <= track_path_index_){
                           *stop_path_index = 0; 
                        }
                    }else{
                        *stop_path_index = 0;
                    }
                    INFO("detected obstacle, stop at "<<(*stop_path_index)<<" path index, track_index:"<<track_path_index_<<" path size:"<<result_.path.size());
                }
                return false;
            }
        }
        return true;

    }

    void VehicleMonitor::check(){

        if(!tracking_ || result_.node_path.empty() || vehicle_ == nullptr || moyingmap_ == nullptr)
            return;

        if(!checked_nav_stop_){    //没有触发导航停止
            
            if( vehicle_->isRobotCanNav() && !emergent_obstacle_detect_){   

                float min_dis = INT32_MAX;
                moyingmap_->getMinDisVehicle(vehicle_,min_dis);

                int stop_path_index = 0;
                if( precheckPathPointValid(&stop_path_index) ){  //可导航且路径点上没有障碍物

                    //临时注释
                    // if( revise_vehicle_path_.checkWhetherNeedRevisePath(track_path_index_) ){
                    //     pathReplan(0,true); //尝试重规划修正路径
                    //      return;
                    // }

                    if(track_path_index_ < result_.path.size() && track_path_index_ >= 0 && vehicle_->deadlock_group_id() == 0){

                        int time_out = path_follow_timeout_ <= 0 ? 4 : path_follow_timeout_;
                        std::chrono::system_clock::time_point last_pt_estimate_time;
                        auto estimate_time = result_.path[track_path_index_].t;
                        auto now = std::chrono::system_clock::now();
                        if(track_path_index_ > 0){
                            last_pt_estimate_time = result_.path[track_path_index_-1].t;
                        }else{
                            last_pt_estimate_time = now;
                        }

                        int diff = std::chrono::duration_cast<std::chrono::seconds>(now - estimate_time).count();
                        int time_need = std::chrono::duration_cast<std::chrono::seconds>(estimate_time-last_pt_estimate_time).count();
                        unsigned int time_diff = 0;

                        bool time_dis_too_much = false;
                        if(diff < 0){
                            time_diff = abs( abs(diff)-abs(time_need) );
                        }else{
                            time_diff = diff;
                        }
                        if(time_diff >= time_out){
                            time_dis_too_much = true;
                        }

                        // if( time_dis_too_much && !checked_nav_stop_ && result_.node_path.size() > 3){   //到达时间差太多就重新规划
                        if( time_dis_too_much && !checked_nav_stop_){   //到达时间差太多就重新规划
                            WARNING("the difference of navigation arrive time is "<<diff<<"s, replan path,robot_id:"<<vehicle_->getID()
                                <<" path_index:"<<track_path_index_<<" cur_time:"<<time(0) 
                                    << " estimate_time:"<<std::chrono::duration_cast<std::chrono::seconds>(estimate_time.time_since_epoch()).count() );
                            
                            if(track_path_index_ < 5)
                                vehicle_->set_nav_fail_times(vehicle_->nav_fail_times()+1);
                            else
                                vehicle_->set_nav_fail_times(vehicle_->nav_fail_times()+1);

                            
                            if(track_path_index_ <= 2){
                                float min_dis = 10000;
                                moyingmap_->getRobotMinDisToObstacle(vehicle_->getID(),min_dis);
                                //
                                if(min_dis < MIN_DIS_OBSTACLE || vehicle_->nav_fail_times() >= ALLOW_MAX_NAV_FAIL_TIMES){
                                    
                                    if( tryEmergencyRecovery() ){ //第一个点超时未到达
                                        stopRobot(0);
                                        pathReplan(time(0)+2);
                                    }else
                                    {
                                        pathReplan(0);
                                    }
                                    
                                }else{
                                    stopRobot(track_path_index_+3);
                                    pathReplan(0);
                                }
                            }else{
                                stopRobot(track_path_index_+3);
                                pathReplan(0);
                            }
                            stopMonitoring();
                        }else{

                            if( !checkOneWayRoadCanPass(result_,track_path_index_) ){    //检测单向是否能通过,不能通过的话,停止当前导航,3秒后重规划路径
                                
                                stopRobot(0);
                                stopMonitoring();
                                pathReplan(time(0)+5);
                            
                            }
                        }
                    }

                    return;
                }else{

                    // stopRobot(stop_path_index);
                    stopRobot(0);
                    
                    WARNING("dectect obstacle in nav path, send stop nav order to robot, robot_id:"<<vehicle_->getID());
                }
            }else{
                stopRobot(0);
                WARNING("robot can't nav, robot_id:"<<vehicle_->getID());
            }

            //清除标志,只处理一次
            emergent_obstacle_detect_ = false;
            //触发导航停止
            checked_nav_stop_ = true;
            check_stop_time_  = std::chrono::system_clock::now();
            return;
        }

        NavModeInfo nav_mode_info = vehicle_->nav_mode_info();
        switch(nav_mode_info.nav_mode){
            case moying_proto::kNavWaitInfinite:
            {
                if( !vehicle_->isRobotCanNav() || !precheckPathPointValid()){    //不可导航或者有障碍物,等待
                    return;
                }else{
                    // updateRestPathPointArriveTime(track_path_index_);
                }
            }
                break;
            case moying_proto::kNavWaitTimeout:
            {
                if( !vehicle_->isRobotCanNav() ){    //不可导航,等待
                    return;
                }else{
                    std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
                    int time_diff = std::chrono::duration_cast<std::chrono::seconds>( now - check_stop_time_).count();
                    INFO("time_diff:"<<time_diff<<" timeout:"<<nav_mode_info.nav_wait_timeout);
                    bool obstacle_disappeared = false;
                    if(time_diff < nav_mode_info.nav_wait_timeout){    //没有超时
                        int collide_path_index=0;
                        if(!precheckPathPointValid(nullptr,&collide_path_index)){    //有障碍物,等待
                            if(collide_path_index != collide_path_index_){  //如果碰撞预测路径点索引发生变化,更新开始等待障碍物移开时间
                                collide_path_index_ = collide_path_index;
                                check_stop_time_  = std::chrono::system_clock::now();
                            }
                            return;
                        }else{
                        //     obstacle_disappeared = true;
                        // }
                        // else{
                            if( updateRestPathPointArriveTime(track_path_index_) ){  //更新到达时刻成功,返回
                                return;
                            }else{
                                WARNING("the obstacle is disappeared before timeout,but update arrive time failed, robot_id:"<<vehicle_->getID()
                                            <<" time:"<<std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count() );
                            }
                        }
                    }else{
                        INFO("timeout before obstacle disappeared, will replan, robot_id:"<<vehicle_->getID()
                                <<" time:"<<std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count() );
                    }
                    
                    float min_dis = 10000;
                    moyingmap_->getRobotMinDisToObstacle(vehicle_->getID(),min_dis);
                    int path_plan_begin_time=0;
                    if(min_dis < 0.3 && tryEmergencyRecovery()){
                            path_plan_begin_time = time(0)+2;
                    }
                    
                    pathReplan(path_plan_begin_time); //添加新的路径规划请求
                    // pathReplan(0); //添加新的路径规划请求
                    stopMonitoring();
                }
            }
                break;
            default:
                break;
        }        
    }

    bool VehicleMonitor::pathReplan(int path_plan_begin_time,bool is_revise){
        
        if(!is_revise && max_replan_times_-- <= 0 ){
            RobotCannotArriveDestNotify notify;
            notify.set_robot_id(vehicle_->getID());
            Pose2D dest;
            dest.set_x(result_.goal.x);
            dest.set_y(result_.goal.y);
            dest.set_theta(result_.goal.th);
            *notify.mutable_dest_pose() = dest;
            notify.set_cause(CannotArriveDestCause::kPathReplanMaxTimes);
            DispatchServer::instance()->notifyRobotCannotArrive(notify);
            max_replan_times_ = MAX_REPLAN_TIMES;
            return false;
        }
            
        //重新请求路径规划
        navigation::VehicleState start_state, goal_state;
        vehicle_->getPose(start_state);
        goal_state = result_.goal;

        if( !is_revise )
            moyingmap_->clearVehiclePath(vehicle_);
        

        PathPlanningRequest path_plan_req = PathPlanningRequest(vehicle_, start_state, goal_state,path_plan_begin_time);
        auto global_goal = vehicle_->global_goal();
        if(fabs(goal_state.x-global_goal.x)+fabs(goal_state.y-global_goal.y)<0.05 && fabs(goal_state.th-global_goal.th)<0.01){
            path_plan_req.is_global_goal = true;
        }else{
            path_plan_req.is_global_goal = false;
        }
        path_plan_req.occupy_time_after_arrive = result_.occupy_time_after_arrive;
        path_plan_req.is_revise_path = is_revise;
        bool ret = DispatchServer::instance()->addPathPlanRequest(vehicle_->getID(),path_plan_req);
        INFO("add path replan start_x:"<<start_state.x<<" start_y:"<<start_state.y<<" start_theta:"<<start_state.th<<
            " goal_x:"<<goal_state.x<<" goal_y:"<<goal_state.y<<" goal_theta:"<<goal_state.th <<" robot_id:"<<vehicle_->getID());
        path_replaning_ = ret;
        if(!ret){
            RobotCannotArriveDestNotify notify;
            notify.set_robot_id(vehicle_->getID());
            Pose2D dest;
            dest.set_x(result_.goal.x);
            dest.set_y(result_.goal.y);
            dest.set_theta(result_.goal.th);
            *notify.mutable_dest_pose() = dest;
            notify.set_cause(CannotArriveDestCause::kPathReplanFailed);
            DispatchServer::instance()->notifyRobotCannotArrive(notify);
        }
        return ret;
    }

    bool VehicleMonitor::updateRestPathPointArriveTime(int rest_start_index){

        //不更新到达时刻了,直接重新规划
        return false;

        if(moyingmap_ == nullptr || result_.path.empty())
            return false;

        std::chrono::system_clock::time_point map_cur_time = moyingmap_->current_time();
        int size = result_.node_path.size();
        int time_index = 4;
        std::vector<Node> new_node_path;
        std::vector<VehicleState> new_path;

        int origin_t = 0;
        if(rest_start_index>0){
            origin_t = result_.node_path[rest_start_index-1].t;
        }
        
        for(int i = rest_start_index; i < size; i++,time_index++)
        {
            time_index = result_.node_path[i].t - origin_t;
            result_.node_path[i].t = time_index;
            bool is_free = true;
            if(i > rest_start_index){
                is_free = moyingmap_->isFree(vehicle_,&result_.node_path[i-1],&result_.node_path[i]);
                if(!is_free)
                    return false;
            }

            // if(i == rest_start_index){  //更新时间地图时，跳过下一个点
            //     continue;
            // }
            result_.path[i].t = map_cur_time + std::chrono::milliseconds( int(time_index*moyingmap_->time_resolution()*1000) );   
            new_node_path.emplace_back(result_.node_path[i]);
            new_path.emplace_back(result_.path[i]);
        }

        if(new_path.empty())
            return false;

        moyingmap_->clearVehiclePath(vehicle_);

        result_.node_path   = new_node_path;
        result_.path        = new_path;
        result_.goal        = result_.goal;
        result_.goal.t      = new_path.end()->t;
        result_.start_time  = map_cur_time;
        result_.goal_time   = result_.goal.t;

        moyingmap_->occupyTimeMap(vehicle_,result_);

        bool ret = DispatchServer::instance()->updateVehiclePathResult(vehicle_->getID(),result_);
        notifyPath();
        track_path_index_ = 0;
        checked_nav_stop_ = false;
        if(ret){
            INFO("nav path arrive time updated successfully, publish new path data,robot_id:"<<vehicle_->getID()
                    <<" goal_x:"<<result_.goal.x<<" goal_y:"<<result_.goal.y<<" goal_th:"<<result_.goal.th <<" path size:"<<result_.path.size());
        }
        return ret;

    }

    void VehicleMonitor::startMonitoring(navigation::PlanningResult &result)
    {
        printf("!!!!! start monitoring\n");
        // moyingmap_->clearVehiclePath(vehicle_);
        result_.clear();
        moyingmap_->occupyTimeMap(vehicle_,result);
        result_ = result;

        checked_nav_stop_ = false;
        track_path_index_ = 0;
        tracking_ = true;
        path_replaning_ = false;

    }
  
    bool VehicleMonitor::clearTimemapFootprint(){
        if(!path_replaning_)
            return false;
        path_replaning_ = false;

        if(vehicle_ == nullptr)
            return false;
        moyingmap_->clearVehiclePath(vehicle_);
        result_.clear();
        return true;
    }
    

    void VehicleMonitor::localObstacleCallback(const moying_proto::LocalObstacle &local_obstacle)
    {
        if( (local_obstacle.width() == 0 || local_obstacle.height() == 0) && local_obstacle.data().size() != 0)
            return;

        if(moyingmap_){

            LocalObstacleInfo obs_info;
            obs_info.last_recv_time = std::chrono::system_clock::now();
            obs_info.raw_obstacle = local_obstacle;
            obs_info.temp_obstacle = local_obstacle;
            obs_info.temp_obstacle.clear_data();
            obs_info.last_recv_time = std::chrono::system_clock::now();

            auto local_obstacle_ptr = &obs_info.raw_obstacle;
            float raw_res = costmap_->getRawResolution();
            int raw_map_cell_x_size = costmap_->getRawSizeInCellsX();
            int raw_map_cell_y_size = costmap_->getRawSizeInCellsY();
            int min_x,min_y,max_x,max_y;
            min_x=min_y=max_x=max_y=0;

            float robot_x = vehicle_->getPose().x;
            float robot_y = vehicle_->getPose().y;
            float h_rl = vehicle_->getLength()/2;
            float h_rw = vehicle_->getWidth()/2;
            
            float half_diagonal = h_rw;
            obs_info.min_obstacle_dis = 10000;

            for(auto obs : local_obstacle_ptr->data()){
                float x = local_obstacle_ptr->origin_x() + obs.x()*raw_res;
                float y = local_obstacle_ptr->origin_y() + obs.y()*raw_res;
                int rmx,rmy;
                int delta = 6;
                costmap_->worldToRawMap(x,y,rmx,rmy);
                int minx = rmx-delta < 0 ? 0 : rmx-delta;
                int maxx = rmx+delta >= raw_map_cell_x_size ? raw_map_cell_x_size-1 : rmx+delta;
                int miny = rmy-delta < 0 ? 0 : rmy-delta;
                int maxy = rmy+delta >= raw_map_cell_y_size ? raw_map_cell_y_size-1 : rmy+delta;

                //判断点周围邻近距离内如果有一个是障碍物点,就认为是地图障碍物
                bool map_obstacle = false;
                for(int x=minx;x<=maxx;x++){
                    for(int y=miny;y<=maxy;y++){
                        if( (*costmap_->get_raw_cost_map())[x][y] == MAPCOSTS::OBSTACLE ){
                            map_obstacle = true;
                            break;
                        }
                    }
                    if(map_obstacle)
                        break;
                }
                if(!map_obstacle){
                    *obs_info.temp_obstacle.add_data() = obs;
                    min_x = obs.x() < min_x ? obs.x() : min_x;
                    min_y = obs.y() < min_y ? obs.y() : min_y;
                    max_x = obs.x() > max_x ? obs.x() : max_x;
                    max_y = obs.y() > max_y ? obs.y() : max_y;
                }

                float x_dis = x - robot_x;
                float y_dis = y - robot_y;
                float dis = sqrt(x_dis*x_dis + y_dis*y_dis);
                float dis2 = dis - half_diagonal;
                dis2 = dis2 < 0 ? 0 : dis2;
                obs_info.min_obstacle_dis = dis2 < obs_info.min_obstacle_dis ? dis2 : obs_info.min_obstacle_dis;

            }

            // printf("min obstacle dis : %.2f robot_id:%d\n",obs_info.min_obstacle_dis,vehicle_->getID());

            obs_info.temp_obstacle.set_origin_x(obs_info.temp_obstacle.origin_x() + min_x*costmap_->getRawResolution());
            obs_info.temp_obstacle.set_origin_y(obs_info.temp_obstacle.origin_y() + min_y*costmap_->getRawResolution());
            obs_info.temp_obstacle.set_width(max_x-min_x+1);
            obs_info.temp_obstacle.set_height(max_y-min_y+1);
            for(auto& obs : *obs_info.temp_obstacle.mutable_data()){
                obs.set_x(obs.x()-min_x);
                obs.set_y(obs.y()-min_y);
            }

            sch::ConfigInfo& cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
            // inflateObstacle(cfg_info.traffic_manager_cfg.static_map_inflate,obs_info.temp_obstacle,obs_info.occupancy_cells);
            if(vehicle_->deadlock_group_id() > 0)
                inflateObstacle(0.0,obs_info);  //如果在死锁组中,不要膨胀障碍物了
            else
            {
                inflateObstacle(0.2,obs_info);
            }
            

            //把当前有路径的机器人的足迹，从局部障碍物中去除
            std::vector<navigation::FootprintCell> has_path_robot_footprints;
            moyingmap_->ergodicRobots([this,&obs_info,&has_path_robot_footprints](VehiclePtr vehicle)->bool{

                if(vehicle == nullptr || vehicle->getID() == vehicle_->getID())
                    return true;
                auto path_plan_result = vehicle->path_plan_result();
                if( path_plan_result.path.empty() )
                    return true;

                FootprintOnMap footprint;
                if( vehicle->getFootprintOccupiedCells(moyingmap_->getCostMap(),footprint) ){
                    for(auto item : footprint){
                        // obs_info.occupancy_cells
                       has_path_robot_footprints.emplace_back(item.second);
                    }
                }
                return true;
            });

            std::vector<std::pair<int,int>> occus;
            for(auto item : obs_info.occupancy_cells){
                bool remove = false;
                for(auto footprint : has_path_robot_footprints){
                    if(footprint.x == item.first && footprint.y == item.second){

                        auto robot_pose = vehicle_->getPose();
                        double wx,wy;
                        if( moyingmap_->getCostMap()->mapToWorld(item.first,item.second,wx,wy) ){
                            float x_delta = robot_pose.x-wx;
                            float y_delta = robot_pose.y-wy;
                            float dis = sqrt(x_delta*x_delta+y_delta*y_delta);
                            if(dis > 2){
                                remove = true;
                                break;
                            }
                        }
                        break;
                    }
                }
                if(remove)
                    continue;
                occus.emplace_back(item);
            }
            obs_info.occupancy_cells = occus;
  
            // INFO("===temp obstacle size:"<<obs_info.temp_obstacle.data_size());
            viz_->publishLocalObstacle(obs_info.temp_obstacle,true);
            viz_->publishObstacleOccupancy(obs_info.occupancy_cells);
            moyingmap_->updateLocalObstacle(vehicle_->getID(),obs_info);
            
        }

        viz_->publishLocalObstacle(local_obstacle);
        
        // publishVisualization();
    }

    void VehicleMonitor::inflateObstacle(double inflation_radius,LocalObstacleInfo& obs_info)
    {

        std::vector<std::pair<int,int>>& map_cells = obs_info.occupancy_cells;
        const moying_proto::LocalObstacle& local_obs = obs_info.temp_obstacle;
        float raw_resolution_ = moyingmap_->getCostMap()->getRawResolution();
        int inflation_num = round(inflation_radius/raw_resolution_);
        int k,l;

        int min_x,min_y,max_x,max_y;
        min_x=min_y=INT32_MAX;
        max_x=max_y=INT32_MIN;

        for(auto obs : local_obs.data())
        {
            int x_edge_down = obs.x() - inflation_num;
            int x_edge_up = obs.x() + inflation_num;
            int y_edge_down = obs.y() - inflation_num;
            int y_edge_up = obs.y() + inflation_num;

            float o_x = local_obs.origin_x();
            float o_y = local_obs.origin_y();
            for(k = x_edge_down; k <= x_edge_up; k++)
            {
                for(l = y_edge_down; l <= y_edge_up; l++)
                {
                    
                    float wx = o_x + k * raw_resolution_;
                    float wy = o_y + l * raw_resolution_;
                    int mx,my;
                    // world_coors.push_back(std::make_pair(x,y));
                    moyingmap_->getCostMap()->worldToMap(wx,wy,mx,my);
                    bool contained = false;
                    for(auto cell :map_cells){
                        if(cell.first == mx && cell.second == my){
                            contained = true;
                            break;
                        }
                    }
                    if(!contained){
                        map_cells.emplace_back(std::make_pair(mx,my));
                        min_x = mx < min_x ? mx : min_x;
                        min_y = my < min_y ? my : min_y;
                        max_x = mx > max_x ? mx : max_x;
                        max_y = my > max_y ? my : max_y;
                    }
                }
            }
        }
        obs_info.max_cell_x = max_x;
        obs_info.max_cell_y = max_y;
        obs_info.min_cell_x = min_x;
        obs_info.min_cell_y = min_y;
    }
    

    bool VehicleMonitor::isPointValid(unsigned int point_index)
    {
        if(result_.path.empty())
            return false;

        if(point_index >= result_.path.size())
            point_index = result_.path.size()-1;

        int mx, my;
        mx = result_.node_path[point_index].x;
        my = result_.node_path[point_index].y;
        float wx,wy;
        wx = result_.path[point_index].x;
        wy = result_.path[point_index].y;
        sch::ConfigInfo& cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
        float l_padding = cfg_info.traffic_manager_cfg.robot_length_padding;
        float w_padding = cfg_info.traffic_manager_cfg.robot_width_padding;

        std::vector<Coordinate> coor_vec;
        //先用高分辨率的局部障碍物做碰撞检测 
        // if(moyingmap_->checkCollideLocalObstacle(vehicle_->getID(),mx,my,result_.path[point_index].th,&coor_vec,true,l_padding,w_padding)){
        //     return false;
        // }

        bool check_robot_occupancy = false;
        if(moyingmap_->checkCollideLocalObstacleCell(vehicle_->getID(),mx,my,check_robot_occupancy)){
            return false;
        }
        
        return true;

    }

    bool VehicleMonitor::checkOneWayRoadCanPass(const navigation::PlanningResult &result,int track_path_index){

        if(vehicle_ == nullptr || moyingmap_ == nullptr){
            return false;
        }
        
        int precheck_count = round(precheck_one_way_road_can_pass_len_ / moyingmap_->getCostMap()->getResolution());

        for(int i=0;i<precheck_count;i++){
            int index = track_path_index+i;
            if(index >= result.node_path.size())
                break;
            
            const Node& node = result.node_path[index];
            int mx = node.x;
            int my = node.y;
            
            auto all_owr = moyingmap_->all_one_way_road();
            for(auto owr : *all_owr){
                if(owr->isCellInRoad(MapCell(mx,my))){
                    
                    if( !owr->canEnterRoad(Orientation(node.th),vehicle_) ){
                        owr->increaseRefuseEnterTimes();
                        WARNING("precheck one way road cann't pass, robot_id:"<<vehicle_->getID()<<" road_id:"<<owr->id());
                        // owr->printfInfo();
                        return false;
                    }
                }
            }
        }
        
        return true;
    }

    void VehicleMonitor::notifyArrived(const moying_proto::NavStateNotify &notify)
    {
        unsigned long long arrived_time = notify.state_info().timestamp();
        unsigned int reached_index = notify.state_info().path_index();

        emergent_obstacle_detect_ = notify.state_info().state() == moying_proto::NavState::kNavEmergentObs;

        if(notify.state_info().state() == moying_proto::NavState::kNavEmergentObs){
            WARNING("emergent obstacle appeared, robot_id:"<<vehicle_->getID());
            tryEmergencyRecovery();
            stopMonitoring();
            pathReplan(time(0)+2);
            return;
        }
        
        // INFO("recv robot arrive , track index:"<<track_path_index_<<" arrive index:"<<reached_index);
        switch (notify.state_info().state())
        {
            case NavState::kArriveStepDestPos:
            {
                if(reached_index >= 2){
                    int times = vehicle_->nav_fail_times();
                    vehicle_->set_nav_fail_times(times > 0 ? times-1 : 0);
                }
                if(track_path_index_ != reached_index){
                    WARNING("Wrong monitoring index, tracking "<<track_path_index_<<" but got "<<reached_index<<" robot_id:"<<vehicle_->getID());
                    if( reached_index < result_.node_path.size()-1 ){
                        track_path_index_ = reached_index+1;
                    }
                }else{

                    track_path_index_ = reached_index + 1;
  
                    // if(result_.path.size() > 0 && reached_index < result_.path.size()-1){
                    //     auto estimate_time = result_.path[reached_index].t;
                    //     auto now = std::chrono::system_clock::now();
                    //     int diff = std::chrono::duration_cast<std::chrono::seconds>(now - estimate_time).count();
                    //     if(abs(diff) >= 3 && !checked_nav_stop_){   //到达时间差太多就重新规划
                    //         WARNING("the difference of navigation arrive time is "<<diff<<"s, replan path,robot_id:"<<vehicle_->getID()
                    //             <<" path_index:"<<reached_index<<" cur_time:"<<time(0));
                    //         stopRobot(track_path_index_+2);
                    //         pathReplan();
                    //         stopMonitoring();
                    //         break;
                    //     }
                    // }
                    
                }
            }
            break;
            case NavState::kArriveFinalDestPos:
            {
                stopMonitoring();   
                max_replan_times_ = MAX_REPLAN_TIMES;

                auto all_owr = moyingmap_->all_one_way_road();
                for(auto owr : *all_owr){
                    if(owr->isVehicleInRoad(vehicle_)){
                        
                        if( owr->getVehicleCountInRoad() == 1 ){
                            owr->set_cur_orientation(Orientation::kUndefine);
                            return;
                        }else{
                            break;
                        }
                    }
                }

            }
                break;
            case NavState::kNavCanceled:
            case NavState::kNavFailed:
            {
                
                vehicle_->set_nav_fail_times(vehicle_->nav_fail_times()+1);
                if(vehicle_->nav_fail_times() >= ALLOW_MAX_NAV_FAIL_TIMES){
                    tryEmergencyRecovery();
                    stopMonitoring();
                    pathReplan(time(0)+2);
                }
            }
                break;
            default:
                break;
        }

    }

    void VehicleMonitor::stopMonitoring()
    {
        tracking_ = false;
        checked_nav_stop_ = false;
        track_path_index_ = 0;
        // result_.clear();
    }

     bool VehicleMonitor::cancelNav(){

         stopRobot(0);
         stopMonitoring();
         moyingmap_->clearVehiclePath(vehicle_);
         return true;

     }

    void VehicleMonitor::stopRobot(int stop_path_index)
    {
        DispatchServer::instance()->handleSendStopToRobot(vehicle_->getID());
        if(stop_path_index == 0){
            INFO("send stop to robot:"<<vehicle_->getID());
            moying_proto::StopNavService_Request req;
            req.set_robot_id(vehicle_->getID());
            SystemManagerCommuClient::instance()->sendStopNavToRobot(req);
            return;
        }

        moying_proto::PathPlanDataNotify body;
        body.set_robot_id(vehicle_->getID());
        body.set_map_name("");

        for(int index = track_path_index_; index <= stop_path_index ; index++){
            if( index >= result_.path.size() )
                break;
            VehicleState state = result_.path[index];
            double x = state.x;
            double y = state.y;
            double th = state.th;
            unsigned long long t = std::chrono::duration_cast<std::chrono::milliseconds>(state.t.time_since_epoch()).count();

            moying_proto::PathPoint point;
            point.mutable_pose()->set_x(x);
            point.mutable_pose()->set_y(y);
            point.mutable_pose()->set_theta(th);
            point.set_timestamp(t);
            point.set_pt_index(index);
            point.set_property(1);
            if(index < stop_path_index){
                point.set_vx(state.vel_x);
                point.set_vy(state.vel_y);
                point.set_vth(state.vel_th);
            }
            *body.add_pt() = point;

            INFO("add stop path index:"<<index);
        }
        
        path_publisher_->publish(path_pub_topic_, body);
    }

    void VehicleMonitor::notifyPath()
    {
        moying_proto::PathPlanDataNotify body;
        body.set_robot_id(vehicle_->getID());
        body.set_map_name("");
        std::vector<moying_proto::PathPoint> points;
        DispatchServer::instance()->getProtoPath(result_,points);
        for(auto& point : points)
        {
            *body.add_pt() = point;
        }

        body.set_follow_mode(0);
        ConfigInfo& cfg_info = ScheduleConfig::instance()->getConfigInfo();
        body.set_nav_speed(cfg_info.traffic_manager_cfg.robot_nav_speed);

        path_publisher_->publish(path_pub_topic_, body,false);
        viz_->publishTrajectory(result_);
    }

    bool VehicleMonitor::tryEmergencyRecovery(){
// return false;
        auto now = std::chrono::system_clock::now();
        if(now < last_try_recovery_time_+std::chrono::seconds(5)){
            WARNING("you just tried emergency recovery, wait a moment...");
            return false;
        }
        last_try_recovery_time_ = now;

        LocalObstacleInfo obs_info;
        bool local_obs_expired = true;
        if( moyingmap_->getLocalObstacle(vehicle_->getID(),obs_info) ){
            if(now < obs_info.last_recv_time+std::chrono::seconds(2)){
                local_obs_expired = false;
            }
        }

        if(local_obs_expired){
            WARNING("not recv local obstacle, wait a moment...");
            return false;
        }

        float recovery_distance_ = 0.15; //脱离距离
        float mov_speed = 0.15;  //脱离的速度

        for(int i=0;i<3;i++){
            if(i > 0)
                recovery_distance_ += 0.1;
        
            std::vector<double> robot_pose;
            robot_pose.push_back(vehicle_->getPose().x);
            robot_pose.push_back(vehicle_->getPose().y);
            robot_pose.push_back(vehicle_->getPose().th);

            std::pair<float,float> offsets_omni[] = {{-1,0},{-1,-1},{-1,1},{0,1},{0,-1},{1,-1},{1,1},{1,0}};
            std::pair<float,float> offsets_diff[] = {{-1,0},{1,0}};
            float recovery_theta_omni[] = {M_PI, -M_PI_4*3, M_PI_4*3, M_PI_2, -M_PI_2, -M_PI_4, M_PI_4, 0};
            float recovery_theta_diff[] = {M_PI, 0};

            /***选择避让点***/
            std::vector<std::vector<double>> recovery_poses;
            if(std::dynamic_pointer_cast<MecanumVehicle>(vehicle_) != nullptr){//全向
                for(auto offset : offsets_omni){
                    std::vector<double> rp = robot_pose;
                    float x_th = robot_pose[2];
                    float y_th = x_th+M_PI_2;
                    if(y_th > M_PI){
                        y_th = y_th - 2*M_PI;
                    }
                    rp[0] +=  offset.first*recovery_distance_*cos(x_th);
                    rp[0] +=  offset.second*recovery_distance_*cos(y_th);
                    rp[1] +=  offset.first*recovery_distance_*sin(x_th);
                    rp[1] +=  offset.second*recovery_distance_*sin(y_th);
                    recovery_poses.emplace_back(rp);
                    // printf("robot:%d : recovery pose x:%.2f,y:%.2f,th:%.2f\n",rp[0],rp[1],rp[2],vehicle_->getID());
                }
            }else if(std::dynamic_pointer_cast<DiffDriveVehicle>(vehicle_) != nullptr){

                for(auto offset : offsets_diff){
                    std::vector<double> rp = robot_pose;
                    float x_th = robot_pose[2];
                    float y_th = x_th+M_PI_2;
                    if(y_th > M_PI){
                        y_th = y_th - 2*M_PI;
                    }
                    rp[0] +=  offset.first*recovery_distance_*cos(x_th);
                    rp[0] +=  offset.second*recovery_distance_*cos(y_th);
                    rp[1] +=  offset.first*recovery_distance_*sin(x_th);
                    rp[1] +=  offset.second*recovery_distance_*sin(y_th);
                    recovery_poses.emplace_back(rp);
                    // ROS_INFO("robot:%d : recovery pose x:%.2f,y:%.2f,th:%.2f",rp[0],rp[1],rp[2],vehicle_->getID());
                }
            }
            /******/

            /***选择最优避让点***/
            float min_obs_dis = INT32_MIN;
            std::vector<double> pose;
            float recovery_theta=0;//机器人移动的朝向,机器人坐标系
            bool can_recovery = false;
            int best_index = 0;
            sch::ConfigInfo& cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
            float l_padding = cfg_info.traffic_manager_cfg.robot_length_padding;
            float w_padding = cfg_info.traffic_manager_cfg.robot_width_padding;
            for(int i=0;i<recovery_poses.size();i++){
                pose = recovery_poses[i];
                //判断是否会和局部障碍物碰撞
                if(moyingmap_->checkCollideLocalObstacle(vehicle_->getID(),pose[0],pose[1],pose[2],nullptr,l_padding,w_padding,false,true)){ 
                    continue;
                }
                //判断是否在最近一段时间会和其他机器人路径有冲突
                if( !moyingmap_->isVehicleFreeLocateCellInRecentSeconds(vehicle_->getID(),pose[0],pose[1],15) ){
                    continue;
                }

                //检测和局部障碍物的距离
                auto local_obs_ = vehicle_->obstacle_info().raw_obstacle;
                float raw_res=moyingmap_->getCostMap()->getRawResolution();
                float min_dis = INT32_MAX;
                for(auto obs : local_obs_.data()){
                    float x = local_obs_.origin_x() + obs.x() * raw_res;
                    float y = local_obs_.origin_y() + obs.y() * raw_res;
                    float x_delta = x-pose[0];
                    float y_delta = y-pose[1];
                    float d = sqrt(x_delta*x_delta+y_delta*y_delta);
                    min_dis = d < min_dis ? d : min_dis;
                }

                //检测和脱离点周围2米内禁区的距离
                int min_mx,min_my,max_mx,max_my;
                moyingmap_->getCostMap()->worldToRawMap(pose[0]-1,pose[1]-1,min_mx,min_my);
                moyingmap_->getCostMap()->worldToRawMap(pose[0]+1,pose[1]+1,max_mx,max_my);
                for(int x=min_mx;x<=max_mx;x++){
                    for(int y=min_my;y<=max_my;y++){
                        if( MAPCOSTS::OBSTACLE == moyingmap_->getCostMap()->getRawCost(x,y) ){
                            double wx,wy;
                            moyingmap_->getCostMap()->rawMapToWorld(x,y,wx,wy);
                            float x_delta = wx-pose[0];
                            float y_delta = wy-pose[1];
                            float d = sqrt(x_delta*x_delta+y_delta*y_delta);
                            min_dis = d < min_dis ? d : min_dis;
                        }
                    }
                }

                printf("robot:%d : min_obs_dis:%.2f x:%.2f y:%.2f theta:%.2f\n",vehicle_->getID(),min_dis,pose[0],pose[1],pose[2]);
                can_recovery = true;
                if(min_dis > min_obs_dis && min_dis != INT32_MAX){
                    min_obs_dis = min_dis;
                    best_index = i;
                }
            }
            if(!can_recovery){
                continue;
            }
            pose = recovery_poses[best_index];

            if(std::dynamic_pointer_cast<MecanumVehicle>(vehicle_) != nullptr){//全向
                if(best_index >= 0 && best_index < sizeof(recovery_theta_omni)/sizeof(recovery_theta_omni[0]) )
                    recovery_theta = recovery_theta_omni[best_index];
            }else if(std::dynamic_pointer_cast<DiffDriveVehicle>(vehicle_) != nullptr){
                if(best_index >= 0 && best_index < sizeof(recovery_theta_diff)/sizeof(recovery_theta_diff[0]) )
                    recovery_theta = recovery_theta_diff[best_index];
            }
            /******/

            printf("select emergency recovery pose, x:%.2f y:%.2f theta:%.2f recovery_theta:%.2f speed:%.2f\n",pose[0],pose[1],pose[2],recovery_theta,mov_speed);
        
            /***执行***/
            std::string js_str;
            try{
                json js;
                js["cmd_type"] = 12;
                json param;
                param["dest_x"] = pose[0];
                param["dest_y"] = pose[1];
                param["dest_theta"] = pose[2];
                param["speed"] = mov_speed;
                param["straight"] = true;
                param["recovery_theta"] = recovery_theta;
                param["recovery_dis"] = recovery_distance_;

                js["param"] = param;
                js_str = js.dump();
            }catch(json::exception e){
                WARNING("send jsonctrl to robot error,json error: "<<e.what());
                return false;
            }
            
            moying_proto::RobotCtrlJsonService_Request r_req;
            r_req.set_robot_id(vehicle_->getID());
            r_req.set_json_str(js_str);
            std::string request_data;
            if(!r_req.SerializeToString(&request_data))
                return false;

            moying_proto::SendRequestToRobotService_Request req;
            req.set_robot_id(vehicle_->getID());
            req.set_robot_req_topic("robot_ctrl_json");
            *req.mutable_request_data() = request_data;
            SystemManagerCommuClient::instance()->sendRequestToRobot(req); 
            HIGHLIGHT("send emergency recovery request to robot, robot_id:"<<vehicle_->getID());
            moyingmap_->clearVehicleFootprint(vehicle_,vehicle_->temp_time_occupy());   //把临时的足迹清除掉,防止机器人移动出原来位置后,时间还被占用
            return true;
        }

        
    }
    
}//namespace navigation
}//namespace moying
